The following code may be helpful in tuning joint PID values.

Place this at the beginning of the GazeboInterface::update function:

    static std::string jointName = "r2/left_leg/joint1";
    static std::string jointName2 = "r2/left_leg/joint1";
    static double jointTarget = GZ_DTOR(50);
    static common::Time startTime = modelPtr->GetWorld()->GetSimTime();
    static std::ofstream fileOut("/tmp/joint.csv");
    static common::Time time = startTime, prevTime;

    static double jointStart =  modelPtr->GetJoint(jointName)->GetAngle(0).GetAsRadian();
    static double jointStart2 =  modelPtr->GetJoint(jointName2)->GetAngle(0).GetAsRadian();
    static bool init = true;
    if (init)
    {
        init = false;
        jointControllerPtr->setJointPosTarget(jointName, jointTarget);
        jointControllerPtr->setJointPosTarget(jointName2, jointTarget);
    }

    static double delay = 0;
    time = modelPtr->GetWorld()->GetSimTime();
    if (time - startTime < delay) return;

Place this at the end of the GazeboInterface::update function:

    double jointPos = modelPtr->GetJoint(jointName)->GetAngle(0).GetAsRadian();
    double jointPos2 = modelPtr->GetJoint(jointName2)->GetAngle(0).GetAsRadian();
    double jointErr = (jointTarget-jointPos)/(jointTarget-jointStart);
    double jointErr2 = (jointTarget-jointPos2)/(jointTarget-jointStart2);

    // report time to target
    static bool firstTarget = true;
    if (firstTarget && jointErr <= 0.01)
    {
        firstTarget = false;
        ROS_DEBUG("Time to target for joint1: %f", (modelPtr->GetWorld()->GetSimTime() - startTime - delay).Float());
    }
    static bool firstTarget2 = true;
    if (firstTarget2 && jointErr2 <= 0.01)
    {
        firstTarget2 = false;
        ROS_DEBUG("Time to target for joint2: %f", (modelPtr->GetWorld()->GetSimTime() - startTime - delay).Float());
    }

    // report highest overshoot
    static bool secondTarget = true;
    static double maxOvershoot = -1.5;
    if (secondTarget && !firstTarget)
    {
        if (-jointErr >= maxOvershoot)
        {
            maxOvershoot = -jointErr;
        }
        else
        {
            ROS_DEBUG("Max overshoot for joint1: %f", maxOvershoot);
            secondTarget = false;
        }
    }
    static bool secondTarget2 = true;
    static double maxOvershoot2 = -1.5;
    if (secondTarget2 && !firstTarget2)
    {
        if (-jointErr2 >= maxOvershoot2)
        {
            maxOvershoot2 = -jointErr2;
        }
        else
        {
            ROS_DEBUG("Max overshoot for joint2: %f", maxOvershoot2);
            secondTarget2 = false;
        }
    }

    // find dampout
    static common::Time inTolTime = startTime;
    static bool inTol = false;
    static bool stop = false;
    if (!stop && fabs(jointErr) <= 0.01)
    {
        if (!inTol)
        {
            inTol = true;
            inTolTime = modelPtr->GetWorld()->GetSimTime();
        }
        if (modelPtr->GetWorld()->GetSimTime() - inTolTime > 5)
        {
            stop = true;
            ROS_DEBUG("Joint1 in tolerance at: %f", (inTolTime - startTime - delay).Float());
        }
    }
    else
    {
        inTol = false;
    }
    static common::Time inTolTime2 = startTime;
    static bool inTol2 = false;
    static bool stop2 = false;
    if (!stop2 && fabs(jointErr2) <= 0.01)
    {
        if (!inTol2)
        {
            inTol2 = true;
            inTolTime2 = modelPtr->GetWorld()->GetSimTime();
        }
        if (modelPtr->GetWorld()->GetSimTime() - inTolTime2 > 5)
        {
            stop2 = true;
            ROS_DEBUG("Joint2 in tolerance at: %f", (inTolTime2  - startTime - delay).Float());
        }
    }
    else
    {
        inTol2 = false;
    }

    if (time - prevTime >= 0.01)
    {
        prevTime = time;
        physics::JointPtr jPtr = modelPtr->GetJoint(jointName);
        physics::JointPtr jPtr2 = modelPtr->GetJoint(jointName2);
        fileOut << modelPtr->GetWorld()->GetSimTime().Double()-delay << ", " << jPtr->GetAngle(0).GetAsRadian() << ", " << jPtr2->GetAngle(0).GetAsRadian() << "\n";
    }

